/* **************************************************************** */
/* Authors:     Jasper Henne, Shana Van Dessel & Cedric Wens        */
/* Document:    Robot.cpp                                           */
/* Description: Verbinden met en aansturen van robot                */
/* **************************************************************** */

#include "Robot.h"
#define DEBUG 1

int Robot::connect(){                   // connectie opzetten
    //signal(SIGINT, stoppen);

    if ((fd = serialOpen("/dev/ttyUSB0", 38400)) == -1)
    {
        cout << "Unable to open serial device: " << strerror << "(" << errno << ")" << endl;
        return 1 ;
    }

    if (wiringPiSetup () == -1)
    {
        cout << "Unable to start wiringPi: " << strerror << "(" << errno << ")" << endl;
        return 1 ;
    }

    delay(1000);
    releaseCan();

    sensor();

    cout << "Connected to robot"<< endl;
    return 0;
}

void Robot::disconnect(){                   // connectie afbreken
    serialClose(fd);
    cout << "Disconnected from robot"<< endl;
}

void Robot::command(char x[])		        // Ccommando doorsturen naar robot
{
        int i;

        for(i=0 ; x[i]!='\0' ; i++)
        {
                serialPutchar(fd, x[i]);
        }

        serialPutchar(fd, 10);
        serialPutchar(fd, 13);
}

/*
void stoppen(int signo)                     // [not used]
{
        char woord[20];
        sprintf(woord,"q 0 0");
        command(woord);
        printf(" noodstop! \n");
        exit(0);
}
*/

void Robot::wheels(int speedl, int speedr){ // wielen bedienen
        char woord[20];
        sprintf(woord,"q %d %d", speedl, speedr);
        command(woord);
}

void Robot::claw(bool close){                // grijper bedienen
        char woord[20];

        if(close)
        {
            sprintf(woord,"g 0");
        }
        else
        {
            sprintf(woord,"g 1");
        }

        int i;
        for(i=0; i<3; i++)
        {
                command(woord);
                delay(5);
        }
}

int Robot::sensor()             // [not used] sensor uitlezen
{
        char woord[20];
        serialFlush(fd);            // eerst serial clearen
        sprintf(woord,"u");
        command(woord);             // nieuwe sensor data aanvragen
        int i=0;
        delay(1000);
        if(serialDataAvail(fd)>0)   // serial data uitlezen
        {
                while(serialDataAvail(fd)!=0)
                {
                        woord[i]=serialGetchar(fd);
                        i++;
                }
                        woord[i]='\0';
        }

        char intje[4];
        i=0;

        while(woord[i] != '=')
        {
                i++;
        }
        i++;
        int j=0;
        while(woord[i] != '\0')
        {
                intje[j]=woord[i];
                i++;
                j++;
        }
        intje[i]='\0';

        #ifdef DEBUG
			cout << "Waarde afstandsensor = " << intje << endl;
		#endif

        return atoi(intje);
}

void Robot::drive(int speed, int tijd){         // rechtdoor rijden
	wheels(speed,speed);
	delay(tijd);
    wheels(0,0);
    //usleep(tijd);
}

void Robot::rotateRight(int tijd){              // rechts draaien
    wheels(50,-50);
    delay(tijd);
    wheels(0,0);
    //usleep(tijd);
}
void Robot::rotateLeft(int tijd){               // links draaien
    wheels(-50,50);
    delay(tijd);
    wheels(0,0);
    //usleep(tijd);
}

void Robot::catchCan(void){                     // grijper sluiten
        claw(true);
}
void Robot::releaseCan(void){                   // grijper openen
        claw(false);
}

void Robot::whereTo(int distance, int angle){
    this->setDistance(distance);                // distance en rotation wijzigen
    this->setRotation(angle);

    #ifdef DEBUG
		cout << "WHERE TO" << endl;
        cout << "distance = " << getDistance() << ", rotation = " << getRotation() << endl << endl;
	#endif
}

void Robot::moveIt(bool dichtbij){

    int tijd = DRAAIFACTOR*abs(getRotation());  // draaitijd berekenen

    #ifdef DEBUG
        cout << "MOVE IT" << endl;
        cout << "hoek = " << getRotation() << ", draaifactor = " << DRAAIFACTOR << ", draaitijd = " << tijd << endl << endl;
    #endif

    
    if(getRotation() > 0)                       // rechts draaien
    {
        rotateRight(tijd);
    }
    else if (getRotation() < 0)                 // links draaien
    {
        rotateLeft(tijd);
    }
    
    if(abs(getRotation())<5)
    {
        if(dichtbij)                                // rijdtijd berekenen
            tijd = RIJFACTORTRAAG*distance;
        else
            tijd = RIJFACTOR*distance;

        #ifdef DEBUG
            cout << "afstand = " << getDistance() << "Rijtijd = " << tijd << endl;
        #endif

        if(dichtbij)                                // rijden
            drive(TRAAG, tijd);
        else
            drive(SNEL, tijd);
    }
}

// GETTERS EN SETTERS --------------------------------------------------------------------
void Robot::setDistance(int dist){
    this->distance = dist;
}

int Robot::getDistance(){
    return this->distance;
}

void Robot::setRotation(int ang){
    this->rotation = ang;
}

int Robot::getRotation(){
    return this->rotation;
}

void Robot::setCaught(bool i){
    this->caught = i;
}

bool Robot::getCaught(){
    return this->caught;
}

int Robot::getXPos(){
    return this->xpos;
}

void Robot::setXPos(int xpos){
    this->xpos = xpos;
}

int Robot::getYPos(){
    return this->ypos;
}

void Robot::setYPos(int ypos){
    this->ypos = ypos;
}

int Robot::getAngle(){
    return this->angle;
}

void Robot::setAngle(int angle){
    this->angle = angle;
}
